#pragma once

#include <vector>
#include <mutex>
#include <deque>
#include <queue>
#include <memory>
#include <iostream>
#include <iomanip>
#include <algorithm>
#include <cmath>
#include <random>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>

#include "cartographer/common/time.h"
#include "glog/logging.h"

typedef std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> eglist3d;
typedef std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>> eglist3f;

namespace cartographer
{
    inline std::stringstream &&Stream()
    {
        std::stringstream sss;
        sss.precision(4); // std::setprecision(4)
        sss.setf(std::ios::fixed);
        return std::move(sss);
    }

    inline bool HasNan(const Eigen::Vector3d &data)
    {
        return std::isnan(data(0)) || std::isnan(data(1)) || std::isnan(data(2));
    }

    inline std::string VecToStr(const Eigen::Vector3d &data)
    {
        std::stringstream sss;
        sss << std::setprecision(4) << data(0) << "," << data(1) << "," << data(2);
        return sss.str();
    }

    template<typename T> Eigen::Vector<T, 3> QuatToEuler(const Eigen::Quaternion<T> &rotation)
    {
        const Eigen::Matrix<T, 3, 3> &mat = rotation.toRotationMatrix();
        return mat.eulerAngles(0, 1, 2);
    }

    template<typename T> inline void Print(const char *name, const Eigen::Vector<T, 3> &data)
    {
        std::stringstream sss;
        sss.precision(4); // std::setprecision(4)
        sss.setf(std::ios::fixed);
        sss << name << " " << data(0) << "," << data(1) << "," << data(2) << "\n";
        LOG(INFO) << sss.str();
    }

    template<typename Derived> inline Eigen::Quaternion<typename Derived::Scalar> deltaQ(const Eigen::MatrixBase<Derived> &theta)
    {
        typedef typename Derived::Scalar Scalar_t;
        Eigen::Quaternion<Scalar_t> dq;
        Eigen::Matrix<Scalar_t, 3, 1> half_theta = theta;
        half_theta /= static_cast<Scalar_t>(2.0);
        dq.w() = static_cast<Scalar_t>(1.0);
        dq.x() = half_theta.x();
        dq.y() = half_theta.y();
        dq.z() = half_theta.z();
        return dq;
    }

    template<typename Derived> inline Eigen::Quaternion<typename Derived::Scalar> positify(const Eigen::QuaternionBase<Derived> &q)
    {
        // printf("a: %f %f %f %f", q.w(), q.x(), q.y(), q.z());
        // Eigen::Quaternion<typename Derived::Scalar> p(-q.w(), -q.x(), -q.y(), -q.z());
        // printf("b: %f %f %f %f", p.w(), p.x(), p.y(), p.z());
        // return q.template w() >= (typename Derived::Scalar)(0.0) ? q : Eigen::Quaternion<typename Derived::Scalar>(-q.w(), -q.x(),
        // -q.y(), -q.z());
        return q;
    }

    template<typename Derived> inline Eigen::Matrix<typename Derived::Scalar, 3, 3> skewSymmetric(const Eigen::MatrixBase<Derived> &q)
    {
        Eigen::Matrix<typename Derived::Scalar, 3, 3> ans;
        ans << typename Derived::Scalar(0), -q(2), q(1), q(2), typename Derived::Scalar(0), -q(0), -q(1), q(0), typename Derived::Scalar(0);
        return ans;
    }

    template<typename Derived> inline Eigen::Matrix<typename Derived::Scalar, 4, 4> Qleft(const Eigen::QuaternionBase<Derived> &q)
    {
        Eigen::Quaternion<typename Derived::Scalar> qq = positify(q);
        Eigen::Matrix<typename Derived::Scalar, 4, 4> ans;
        ans(0, 0) = qq.w(), ans.template block<1, 3>(0, 1) = -qq.vec().transpose();
        ans.template block<3, 1>(1, 0) = qq.vec(),
                                    ans.template block<3, 3>(1, 1) =
                                        qq.w() * Eigen::Matrix<typename Derived::Scalar, 3, 3>::Identity() + skewSymmetric(qq.vec());
        return ans;
    }

    template<typename Derived> inline Eigen::Matrix<typename Derived::Scalar, 4, 4> Qright(const Eigen::QuaternionBase<Derived> &p)
    {
        Eigen::Quaternion<typename Derived::Scalar> pp = positify(p);
        Eigen::Matrix<typename Derived::Scalar, 4, 4> ans;
        ans(0, 0) = pp.w(), ans.template block<1, 3>(0, 1) = -pp.vec().transpose();
        ans.template block<3, 1>(1, 0) = pp.vec(),
                                    ans.template block<3, 3>(1, 1) =
                                        pp.w() * Eigen::Matrix<typename Derived::Scalar, 3, 3>::Identity() - skewSymmetric(pp.vec());
        return ans;
    }

    inline void PrintResidual(double *residuals, int i)
    {
        if (!residuals)
            return;
        std::stringstream sss1;
        for (int i = 0; i < 15; i++)
        {
            sss1 << residuals[i] << ",";
        }
        LOG(INFO) << "resid[" << i << "]" << sss1.str();
    }

    inline void PrintResidual(const Eigen::Matrix<double, 15, 1> &residuals, int i)
    {
        std::stringstream sss1;
        for (int i = 0; i < 15; i++)
        {
            sss1 << residuals(i, 0) << ",";
        }
        LOG(INFO) << "resid[" << i << "]" << sss1.str();
    }

    inline void PrintJacobians(const Eigen::Matrix<double, 15, 15> &jacobians, int i)
    {
        std::stringstream sss;
        for (int i = 0; i < 15; i++)
        {
            for (int m = 0; m < 15; m++)
                sss << jacobians(i, m) << ",";
            sss << "\n";
        }
        LOG(INFO) << "jacobians[" << i << "]\n" << sss.str();
    }

    inline void PrintMatrix(const char *name, const Eigen::MatrixXd &mat)
    {
        auto &&sss = Stream();
        for (int r = 0; r < mat.rows(); r++)
        {
            for (int c = 0; c < mat.cols(); c++)
                sss << mat(r, c) << ",";
            sss << "\n";
        }
        LOG(INFO) << "matrix[" << name << "]\n" << sss.str();
    }

    inline void PrintJacobians(double **jacobians, int i)
    {
        if (!jacobians)
            return;
        std::stringstream sss;
        for (int i = 0; i < 15; i++)
        {
            if (jacobians[i])
            {
                for (int m = 0; m < 15; m++)
                    if (jacobians[i][m])
                        sss << jacobians[i][m] << ",";
                sss << "\n";
            }
            else
            {
                sss << "skip\n";
            }
        }
        LOG(INFO) << "jacobians[" << i << "]\n" << sss.str();
    }

    template<typename Scalar, int Rows> class GaussGenerator
    {
    public:
        GaussGenerator(const Eigen::Matrix<Scalar, Rows, 1> &exp, const Eigen::Matrix<Scalar, Rows, 1> &sigma)
            : exp_(exp)
            , sigma_(sigma)
        {
        }

        GaussGenerator(const Eigen::Matrix<Scalar, Rows, 1> &sigma)
            : sigma_(sigma)
        {
            exp_.setZero();
        }

        Eigen::Matrix<Scalar, Rows, 1> Generate()
        {
            Eigen::Matrix<Scalar, Rows, 1> result;
            for (int i = 0; i < Rows; i++)
            {
                std::normal_distribution<Scalar> dist(exp_(i), sigma_(i));
                result(i) = dist(gen_);
            }
            return result;
        }

        void ResetExp(const Eigen::Matrix<Scalar, Rows, 1> &exp)
        {
            exp_ = exp;
        }

        Eigen::Matrix<Scalar, Rows, 1> exp_;
        Eigen::Matrix<Scalar, Rows, 1> sigma_;
        std::default_random_engine gen_;
    };
} // namespace cartographer